/*!	 valuenode_dynamic.h
**	 Header file for implementation of the "Dynamic" valuenode conversion.
**
**	Copyright (c) 2014 Carlos López
**
**	This package is free software; you can redistribute it and/or
**	modify it under the terms of the GNU General Public License as
**	published by the Free Software Foundation; either version 2 of
**	the License, or (at your option) any later version.
**
**	This package is distributed in the hope that it will be useful,
**	but WITHOUT ANY WARRANTY; without even the implied warranty of
**	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
**	General Public License for more details.
**
*/

#ifndef __SYNFIG_VALUENODE_DYNAMIC_H
#define __SYNFIG_VALUENODE_DYNAMIC_H

#include <synfig/valuenode.h>
#include "valuenode_derivative.h"
#include "valuenode_const.h"

#define MASS_INERTIA_MINIMUM 0.0000001

namespace synfig
{

class Oscillator;

class ValueNode_Dynamic : public LinkableValueNode
{
    friend class Oscillator;
private:

    ValueNode::RHandle tip_static_;    // Equilibrium position without external forces
    ValueNode::RHandle origin_;        // Basement of the dynamic system
    ValueNode::RHandle force_;         // External force applied on the mass center of gravity
    ValueNode::RHandle torque_;        // Momentum applied at the origin
    ValueNode::RHandle damping_coef_;  // Radial Damping coeficient
    ValueNode::RHandle friction_coef_; // Rotational friction coeficient
    ValueNode::RHandle spring_coef_;   // Spring coeficient
    ValueNode::RHandle torsion_coef_;  // Torsion coeficient
    ValueNode::RHandle mass_;          // Mass
    ValueNode::RHandle inertia_;       // Moment of Inertia
    ValueNode::RHandle spring_rigid_;  // True if spring is solid rigid
    ValueNode::RHandle torsion_rigid_; // True if torsion is solid rigid
    ValueNode::RHandle origin_drags_tip_; // If true result=origin+state otherwise result=state

    ValueNode_Derivative::RHandle origin_d_;      // Derivative of the origin along the time
    mutable Time last_time;
    ValueNode_Dynamic(const ValueBase &value);
    /*
    State types (4) for:
    q=radius
    p=d/dt(radius)
    b=angle
    g=d/dt(angle)

    where

    p=dxdt[0]
    p'=dxdt[1]
    g=dxdt[2]
    g'=dxdt[3]
    q=x[0]
    q'=x[1]
    b=x[2]
    b'=x[3]
    */
    mutable std::vector<double> state;
    void reset_state(Time t)const;
public:

    typedef etl::handle<ValueNode_Dynamic> Handle;
    typedef etl::handle<const ValueNode_Dynamic> ConstHandle;

    virtual ValueBase operator()(Time t)const;

    virtual ~ValueNode_Dynamic();

    virtual String get_name()const;
    virtual String get_local_name()const;

    virtual ValueNode::LooseHandle get_link_vfunc(int i)const;

protected:
    LinkableValueNode* create_new()const;
    virtual bool set_link_vfunc(int i, ValueNode::Handle x);

public:
    using synfig::LinkableValueNode::get_link_vfunc;

    using synfig::LinkableValueNode::set_link_vfunc;
    static bool check_type(Type &type);
    static ValueNode_Dynamic* create(const ValueBase &x);
    virtual Vocab get_children_vocab_vfunc()const;
}; // END of class ValueNode_Dynamic

class Oscillator
{
    etl::handle<const ValueNode_Dynamic> d;
public:
    Oscillator(const ValueNode_Dynamic* x) : d(x) { }
    void operator()(const std::vector<double> &x, std::vector<double> &dxdt, const double t)
    {
        Vector u(cos(x[2]), sin(x[2]));
        Vector v(-u[1], u[0]);
        Vector sd = (*(d->origin_d_))(t).get(Vector());
        Vector f = (*(d->force_))(t).get(Vector());
        double to = (*(d->torque_))(t).get(double());
        double c = (*(d->damping_coef_))(t).get(double());
        double mu = (*(d->friction_coef_))(t).get(double());
        double k = (*(d->spring_coef_))(t).get(double());
        double tau = (*(d->torsion_coef_))(t).get(double());
        double m = (*(d->mass_))(t).get(double());
        double i = (*(d->inertia_))(t).get(double());
        Vector tip = (*(d->tip_static_))(t).get(Vector());

        double fr = f * u;
        double fa = f * v;
        // Those are the second derivatives (speed of origin)
        double srd = sd * u;
        double sad = sd * v;
        // Calculate the steady position in terms of state
        double r0 = tip.mag();
        double a0 = (double)(Angle::rad(tip.angle()).get());
        double r = x[0] - r0; // effective radius
        double a = x[2] - a0; // effective alpha
        double rd = x[1]; // radius speed
        double ad = x[3]; // alpha speed
        double imr2 = i + m * x[0] * x[0]; // effective inertia
        // Check if the spring rigid
        bool spring_is_rigid = (*(d->spring_rigid_))(t).get(bool());
        // Check if the torsion rigid
        bool torsion_is_rigid = (*(d->torsion_rigid_))(t).get(bool());
        // Integration operations
        dxdt[0] = x[1];

        // Disable movement if the spring is rigid
        // or if the mass is near to zero but animated.
        if (spring_is_rigid || fabs(m) <= MASS_INERTIA_MINIMUM) {
            dxdt[1] = 0.0;
        } else {
            dxdt[1] = (fr - c * rd - k * r) / m - srd;
        }

        dxdt[2] = x[3];

        // Disable rotation if the torsion is rigid
        // or if the inertia is near to zero but animated.
        if (torsion_is_rigid || fabs(imr2) <= MASS_INERTIA_MINIMUM) {
            dxdt[3] = 0.0;
        } else {
            dxdt[3] = (to + fa * x[0] - mu * ad - tau * a) / imr2 - sad;
        }
    }

};
}; // END of namespace synfig

#endif